{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Discover your Poppy Humanoid"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This notebook will guide you in your very first steps with Poppy Humanoid in Python. \n",
    "\n",
    "What you will see in this notebook:\n",
    "\n",
    "1. Instantiate your robot\n",
    "2. Access motors, send motor commands\n",
    "3. Start high level behaviors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "*We assume here that you are connected to a physical Poppy Humanoid. It also need to be assembled and configured (you can referer to the [documentation](http://docs.poppy-project.org/en) if you haven't done in yet).*\n",
    "<img  src=\"image/poppy_humanoid.jpg\"  width=\"500\"/>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Import some matplolib shortcuts for Jupyter notebook\n",
    "%matplotlib inline\n",
    "from __future__ import print_function\n",
    "\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Instantiate your robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To start using your robot in Python, you first need to instantiate it. You can do that by running the following code:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from pypot.creatures import PoppyHumanoid\n",
    "\n",
    "poppy = PoppyHumanoid()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**If you are using V-REP simulator**, you have to run this code instead. If you have troubles, look at the [documentation to install and run V-REP](http://docs.poppy-project.org/en/installation/install-vrep.html#test-your-installation)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from pypot.creatures import PoppyHumanoid\n",
    "\n",
    "poppy = PoppyHumanoid(simulator='vrep')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This creates a [Robot](http://poppy-project.github.io/pypot/pypot.robot.html#pypot.robot.robot.Robot) object that can be used to access the motors and sensors. It handles all the low-level communication for you, so you do not need to know anything about the serial protocol used to make a motor turn. The *motors* and *sensors* fields of the Robot are automatically synced to match the state of their hardware equivalent."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Before doing anything else, we will initalize everything by asking the robot to go to its init position (the code below will be described in more detailed later):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "poppy.stand_position.start()\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Access motors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In a Poppy Humanoid, motors are defined as illustrated below:\n",
    "\n",
    "<img  src=\"image/poppy_humanoid_motors.png\" width=\"500\"/>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "From the [Robot](http://poppy-project.github.io/pypot/pypot.robot.html#pypot.robot.robot.Robot) object, you can directly retrieve the list of motors connected:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<DxlMotor name=l_elbow_y id=44 pos=-20.0>,\n",
       " <DxlMotor name=r_hip_y id=23 pos=-0.0>,\n",
       " <DxlMotor name=r_knee_y id=24 pos=0.0>,\n",
       " <DxlMotor name=head_y id=37 pos=1.0>,\n",
       " <DxlMotor name=r_ankle_y id=25 pos=-2.0>,\n",
       " <DxlMotor name=r_arm_z id=53 pos=-0.0>,\n",
       " <DxlMotor name=head_z id=36 pos=0.0>,\n",
       " <DxlMotor name=r_shoulder_x id=52 pos=-20.0>,\n",
       " <DxlMotor name=r_shoulder_y id=51 pos=10.0>,\n",
       " <DxlMotor name=r_hip_z id=22 pos=-2.0>,\n",
       " <DxlMotor name=r_hip_x id=21 pos=-2.0>,\n",
       " <DxlMotor name=r_elbow_y id=54 pos=-20.0>,\n",
       " <DxlMotor name=l_arm_z id=43 pos=-0.0>,\n",
       " <DxlMotor name=l_hip_x id=11 pos=2.0>,\n",
       " <DxlMotor name=l_hip_y id=13 pos=0.0>,\n",
       " <DxlMotor name=l_hip_z id=12 pos=2.0>,\n",
       " <DxlMotor name=abs_x id=32 pos=0.0>,\n",
       " <DxlMotor name=abs_y id=31 pos=-2.0>,\n",
       " <DxlMotor name=abs_z id=33 pos=-0.0>,\n",
       " <DxlMotor name=l_ankle_y id=15 pos=-3.0>,\n",
       " <DxlMotor name=bust_y id=34 pos=-0.0>,\n",
       " <DxlMotor name=bust_x id=35 pos=0.0>,\n",
       " <DxlMotor name=l_knee_y id=14 pos=-0.0>,\n",
       " <DxlMotor name=l_shoulder_x id=42 pos=20.0>,\n",
       " <DxlMotor name=l_shoulder_y id=41 pos=10.0>]"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "poppy.motors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "As you can see *poppy.motors* holds a list of all motors."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can retrieve all motors names:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "for m in poppy.motors:\n",
    "    print(m.name)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    " Each of them can be access directly from its name. For instance:\n",
    " \n",
    " "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "poppy.l_elbow_y"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Read values from the motors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "From the motor object you can access its registers. The main ones are:\n",
    "\n",
    "* **present_position**: the current position of the motor in degrees\n",
    "* **present_speed**: the current speed of the motor in degrees per second \n",
    "* **present_load**: the current workload of the motor (in percentage of max load)\n",
    "* **present_temperature**: the temperature of the motor in celsius\n",
    "* **angle_limit**: the reachable limits of the motor (in degrees)\n",
    "\n",
    "They can be accessed directly:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "poppy.l_elbow_y.present_temperature"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Or, to get the present position for all motors:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[-20.0,\n",
       " 0.0,\n",
       " 0.0,\n",
       " 1.0,\n",
       " -2.0,\n",
       " 0.0,\n",
       " 0.0,\n",
       " -20.0,\n",
       " 10.0,\n",
       " -2.0,\n",
       " -2.0,\n",
       " -20.0,\n",
       " -105.0,\n",
       " 2.0,\n",
       " 0.0,\n",
       " 2.0,\n",
       " 0.0,\n",
       " -2.0,\n",
       " 0.0,\n",
       " -3.0,\n",
       " -0.0,\n",
       " 0.0,\n",
       " 0.0,\n",
       " 20.0,\n",
       " 9.700000000000003]"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "[m.present_position for m in poppy.motors]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It's important to understand the *poppy.m1.present_position* is automatically updated with the real motor position (at 50Hz). Similarly for other registers, the update frequency may vary depending on its importance. For instance, the temperature is only refreshed at 1Hz as it is not fluctuating that quickly."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Send motor commands"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "On top of the registers presented above, they are additional ones used to send commands. For instance, the position of the motor is split in two different registers: \n",
    "\n",
    "* the read-only **present_position** of the motor\n",
    "* the read-write **goal_position** which sends to the motor a target position that it will try to reach.\n",
    "\n",
    "If you want to set a new position for a motor, you write:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "poppy.l_arm_z.goal_position = 50"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see the robot turn of 20 degrees. Sending motor command is as simple as that. To make it turn to the other side:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "poppy.l_arm_z.goal_position = -50"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the examples above, the motor turned as fast as possible (its default mode). You can change its *moving_speed* (i.e. its maximum possible speed):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "poppy.l_arm_z.moving_speed = 50"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now the motor *l_arm_z* can not move faster than 50 degrees per second. If we ask to move again, you should see the difference:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "poppy.l_arm_z.goal_position = 90"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The main write registers are:\n",
    "\n",
    "* **goal_position**: target position in degrees\n",
    "* **moving_speed**: maximum reachable speed in degrees per second\n",
    "* **compliant** (explained below) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The dynamixel servo motors have two modes:\n",
    "\n",
    "* **stiff**: the normal mode for motors where they can be controlled\n",
    "* **compliant**: a mode where the motors can be freely moved by hand. This is particularly useful for phyisical human-robot interaction\n",
    "\n",
    "You can make them switch from one mode to the other using the *compliant* register. For instance, you can turn the motor *m6* compliant via:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "poppy.l_arm_z.compliant = True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should now be able to move this motors by hand. This is particularly useful for programming your robot by demonstration (see the dedicated notebook)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    " And to turn it stiff again:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "poppy.l_arm_z.compliant = False"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## High level behaviors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The Poppy Humanoid robot comes with a set of pre-defined behaviors. They can be specific postures - such as the init_position used at the beginning - or a *breathing* motion, ... "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can find the exhaustive list using the *primitives* accessor:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "[p.name for p in poppy.primitives]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Those behaviors (or primitives in \"poppy terms\") can be started, stopped, paused, etc..."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "poppy.upper_body_idle_motion.start()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You can make the Poppy Humanoid *breathe* for 10 seconds:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import time\n",
    "\n",
    "poppy.upper_body_idle_motion.start()\n",
    "time.sleep(10)\n",
    "poppy.upper_body_idle_motion.stop()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Going further\n",
    "\n",
    "Now that you have learnt the basis of what you can do with a Poppy Humanoid, there is much more to discover:\n",
    "* how to record/replay move by demonstration\n",
    "* how to define your own high-level behavior (e.g. a visual servoing of the tip of the robot using blob detection)\n",
    "* used your Poppy Humanoid as a connected device and make it communaticate with the rest of the world using HTTP requests\n",
    "* ...\n",
    "\n",
    "You can find other examples in the [docs](http://docs.poppy-project.org) or in the notebook folder next to this one."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "kernelspec": {
   "display_name": "Python [default]",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}
